#include "viewer.h"

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui.hpp>

namespace pangolin_viewer {

viewer::viewer(std::shared_ptr<Publisher> publisher_ptr)
    : interval_ms_(1000.0f / 30.0f),
      viewpoint_x_(0.0),
      viewpoint_y_(-10.0),
      viewpoint_z_(-0.1),
      viewpoint_f_(2000.0),
      keyfrm_size_(0.1),
      keyfrm_line_width_(1),
      graph_line_width_(1),
      point_size_(2),
      camera_size_(0.15),
      camera_line_width_(2),
      cs_("black"),
      publisher_ptr_(publisher_ptr) {

	follow_camera_ = true;
	mapping_mode_ = true;
	loop_detection_mode_ = true;
	map_viewer_name_	= "PangolinViewer: Map Viewer";
	frame_viewer_name_ = "PangolinViewer: Frame Viewer";
	map_viewer_width_	= 1024;
	map_viewer_height_ = 768;
	terminate_is_requested_ = false;
	is_terminated_ = true;
    // init_pose_status = false;    
    // distance = 0.0;
    // localization_status_ = mpPublisher->localization_status();
    // trajectory_id_ = mpPublisher->trajectory_id();
}

viewer::~viewer() {
	// if (mpPublisher != nullptr) {
	// 	delete mpPublisher;
	// 	mpPublisher = nullptr;
	// }
}

void viewer::run() {
    is_terminated_ = false;

    pangolin::CreateWindowAndBind(map_viewer_name_, 1024, 768);

    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    // depth testing to be enabled for 3D mouse handler
    glEnable(GL_DEPTH_TEST);

#if 1
    // setup camera renderer
    s_cam_ = std::unique_ptr<pangolin::OpenGlRenderState>(new pangolin::OpenGlRenderState(
        pangolin::ProjectionMatrix(map_viewer_width_, map_viewer_height_, viewpoint_f_, viewpoint_f_,
                                   map_viewer_width_ / 2, map_viewer_height_ / 2, 0.1, 1e6),
        pangolin::ModelViewLookAt(viewpoint_x_, viewpoint_y_, viewpoint_z_, 0, 0, 0, 0.0, -1.0, 0.0)));
#else 
	// Define Camera Render Object (for view / scene browsing)
	s_cam_ = std::unique_ptr<pangolin::OpenGlRenderState>(new pangolin::OpenGlRenderState(
		pangolin::ProjectionMatrix(map_viewer_width_, map_viewer_height_, viewpoint_f_, viewpoint_f_, map_viewer_width_ / 2, map_viewer_height_ / 2, 0.1, 10000),
		pangolin::ModelViewLookAt(viewpoint_x_, viewpoint_y_, viewpoint_z_, 0, 0, 0, 1, 0, 0)));
#endif

    // create map window
    pangolin::View& d_cam = pangolin::CreateDisplay()
                                .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -map_viewer_width_ / map_viewer_width_)
                                .SetHandler(new pangolin::Handler3D(*s_cam_));

    // create menu panel
    create_menu_panel();

    // create frame window
    //cv::namedWindow(frame_viewer_name_);

    pangolin::OpenGlMatrix gl_cam_pose_wc;
    gl_cam_pose_wc.SetIdentity();

    //pangolin::View& d_image = pangolin::Display("")
    //  .SetBounds(0., 1/3.0f, 2/3.0f, 1.0, 2.0)
      //.SetLock(pangolin::LockLeft, pangolin::LockTop);
    //  .SetLock(pangolin::LockRight, pangolin::LockBottom);
    //pangolin::GlTexture image_texture(640, 480, GL_RGB, true, 0, GL_RGB, GL_UNSIGNED_BYTE);
    
    while (true) {
        // clear buffer
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        
        // set rendering state
        d_cam.Activate(*s_cam_);
        glClearColor(cs_.bg_.at(0), cs_.bg_.at(1), cs_.bg_.at(2), cs_.bg_.at(3));

        draw_vehicle_poses();
        draw_gt_poses();
        draw_odometry_poses();

		// draw landmarks
        draw_landmarks();
        draw_filter_landmarks();
        draw_current_landmarks();

        draw_predict_poses2d();
        draw_predict_poses3d();

        // draw horizontal grid
        draw_horizontal_grid();

        draw_submap_landmarks();

        draw_submap_poses();

        draw_all_submap_poses();

        draw_trajectory_node_list();
        
        draw_constraint_list();
        
        draw_all_poses();

		draw_particle();

        pangolin::FinishFrame();
       
    }
}

void viewer::create_menu_panel() {
    pangolin::CreatePanel("menu").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(175));
    menu_grid_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Grid", true, true)); 
    menu_show_lms_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show lms", false, true));
    menu_show_veh_pose = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Vehicle Pose", false, true));
    menu_show_gt_pose = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Ground Truth", true, true));
    menu_show_wheel_odometry = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Odometry", true, true));
    menu_show_current_landmark_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Current lm", true, true));
    menu_show_predict_pose2d = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Predict Pose2d", true, true));
    menu_show_predict_pose3d = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Predict Pose3d", true, true));
    menu_show_filter_lms_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Filter lms", true, true));
    menu_show_submap_lms_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Submap lms", false, true));
    menu_show_submap_poses_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Submap poses", true, true));
    
    menu_show_trajectory_node_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Trajectory Node", true, true));

    if(!localization_status_) {
        menu_show_constraint_list_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Constraint List", true, true));
    } else {
        menu_show_constraint_list_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Constraint List", false, true));
    }

    menu_show_all_submap_poses_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Submap landmakrs", true, true));
    menu_show_current_submap_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show Curr Submap", false, true));
    menu_show_all_poses_ = std::unique_ptr<pangolin::Var<bool>>(new pangolin::Var<bool>("menu.Show all poses", false, true));
}

void viewer::draw_horizontal_grid() {
 //    if (!*menu_grid_) {
 //        return;
 //    }

 //    Eigen::Matrix4f origin = Eigen::Matrix4f::Identity();
 //    float trans = 3.1415926f / 180.0f;

 //    float yaw  = 0.0f * trans;
 //    float pitch  = 0.0f * trans;
 //    float roll  = 0.0f * trans;
 //    Eigen::Matrix3f R;
 //    R = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) *
 //        Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
 //        Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX());

 //    origin.block<3, 3>(0, 0) = R;

 //    //origin << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
    
 //    //std::cout << " origin : " << origin << std::endl;

 //    glPushMatrix();
	// //glMultTransposeMatrixf(origin.data());
	// glMultMatrixf(origin.data());

 //    glLineWidth(1);
 //    glColor3fv(cs_.grid_.data());

 //    glBegin(GL_LINES);
 //    const float max = 1500.0f;
 //    const float interval_ratio = 1.0;
 //    const float grid_min = -max * interval_ratio;
 //    const float grid_max = max * interval_ratio;

 //    for (int x = -max / 10; x <= max / 10; x += 1) {
 //        draw_line(x * 10.0f * interval_ratio, grid_min, 0, x * 10.0f * interval_ratio, grid_max, 0);
 //    }
 //    for (int y = -max / 10; y <= max / 10; y += 1) {
 //        draw_line(grid_min, y * 10.0f * interval_ratio, 0, grid_max, y * 10.0f * interval_ratio, 0);
 //    }

 //    glEnd();

 //    glPopMatrix();
}

void viewer::draw_vehicle_poses()
{
 //    if (!*menu_show_veh_pose) {
 //        return;
 //    }
	// std::vector<Eigen::Vector3d> vehicle_poses = mpPublisher->get_poses();
	// if (vehicle_poses.empty()) {
	// 	return;
	// }
 //    glColor3f(1.0f, 1.0f, 0.0f);
 //    glLineWidth(1.0);
 //    glBegin(GL_LINE_STRIP);

 //    for (const auto& pose : vehicle_poses) {
 //        Eigen::Vector3d two = pose;
 //        glVertex3f(two[0], two[1], 0.0);
 //    }

 //    glEnd();
}

void viewer::draw_gt_poses() {
 //    if (!*menu_show_gt_pose) {
 //        return;
 //    }
	// std::vector<Eigen::Vector3d> gt_poses = mpPublisher->get_gt_poses();
	// if (gt_poses.empty()) {
	// 	return;
	// }
 //    glColor3f(0.0f, 1.0f, 0.0f);
 //    glLineWidth(1.0);
 //    glBegin(GL_LINE_STRIP);

 //    for (const auto& pose : gt_poses) {
 //        Eigen::Vector3d two = pose;
 //        glVertex3f(two[0], two[1], 0.0);
 //    }

 //    glEnd();
}

void viewer::draw_odometry_poses() {
    if (!*menu_show_wheel_odometry) {
        return;
    }
	std::vector<Eigen::Vector3d> odom_poses = publisher_ptr_->get_odometry_poses();
	if (odom_poses.empty()) {
		return;
	}
    glColor3f(1.0f, 1.0f, 0.0f);
    glLineWidth(1.0);
    glBegin(GL_LINE_STRIP);

    for (const auto& pose : odom_poses) {
        Eigen::Vector3d two = pose;
        glVertex3f(two[0], two[1], 0.0);
    }

    glEnd();
}

void viewer::draw_all_poses() {
 //    if (!*menu_show_all_poses_) {
 //        return;
 //    }
	// std::vector<Eigen::Vector3d> all_poses = mpPublisher->get_all_poses();
	// if (all_poses.empty()) {
	// 	return;
	// }
 //    glColor3f(1.0f, 1.0f, 0.0f);
 //    glLineWidth(1.0);
 //    glBegin(GL_LINE_STRIP);

 //    for (const auto& pose : all_poses) {
 //        Eigen::Vector3d two = pose;
 //        glVertex3f(two[0], two[1], 0.0);
 //    }

 //    glEnd();
}

void viewer::draw_landmarks() {
    // if (!*menu_show_lms_) {
    //     return;
    // }

    // std::vector<Eigen::Vector3f> landmarks = mpPublisher->get_landmarks();
    
    // if (landmarks.empty()) {
    //     return;
    // }

    // glPointSize(0.1);

    // glColor3fv(cs_.lm_.data());

    // glBegin(GL_POINTS);

    // for (const auto lm : landmarks) {
    //     glVertex3f(lm[0], lm[1], lm[2]);
    // }

    // glEnd();
}

void viewer::draw_filter_landmarks() {
    if (!*menu_show_filter_lms_) {
        return;
    }
    std::vector<Eigen::Vector3f> landmarks = publisher_ptr_->get_filter_landmarks();
    
    if (landmarks.empty()) {
        return;
    }

    glPointSize(1.0);

    glColor3f(1.0f, 1.0f, 0.0f);

    glBegin(GL_POINTS);

    for (const auto lm : landmarks) {
        glVertex3f(lm[0], lm[1], lm[2]);
    }

    glEnd();
}

void viewer::draw_current_landmarks() {
    if (!*menu_show_current_landmark_) {
        return;
    }
    std::vector<Eigen::Vector3f> landmarks = publisher_ptr_->get_current_landmarks();
    
    if (landmarks.empty()) {
        return;
    }

    glPointSize(3.0);

    glColor3f(1.0f, 0.0f, 0.0f);

    glBegin(GL_POINTS);

    for (const auto lm : landmarks) {
        glVertex3f(lm[0], lm[1], lm[2]);
    }

    glEnd();
}

void viewer::draw_predict_poses2d()
{
    if (!*menu_show_predict_pose2d) {
        return;
    }
    if(publisher_ptr_ == nullptr) {
        return;
    }
	std::vector<Eigen::Vector3d> vehicle_poses = publisher_ptr_->get_predict_poses2d();
	if (vehicle_poses.empty()) {
		return;
	}

    glColor3f(1.0f, 1.0f, 1.0f);
    glLineWidth(1.0);
    glBegin(GL_LINE_STRIP);

    for (const auto& pose : vehicle_poses) {
        Eigen::Vector3d two = pose;
        glVertex3f(two[0], two[1], 0.0);
    }

    glEnd();
}

void viewer::draw_predict_poses3d()
{
    if (!*menu_show_predict_pose3d) {
        return;
    }
	std::vector<Eigen::Vector3d> vehicle_poses = publisher_ptr_->get_predict_poses3d();
	if (vehicle_poses.empty()) {
		return;
	}
    glColor3f(0.0f, 1.0f, 1.0f);
    glLineWidth(1.0);
    glBegin(GL_LINE_STRIP);

    for (const auto& pose : vehicle_poses) {
        Eigen::Vector3d two = pose;
        glVertex3f(two[0], two[1], two[2]);
    }

    glEnd();
}

void viewer::draw_submap_landmarks() {
    // if(!*menu_show_submap_lms_) {
    //     return;
    // }

    // //std::vector<Eigen::Vector3d> landmarks = mpPublisher->get_submap_landmarks();
    // std::vector<Eigen::Vector3d> landmarks = mpPublisher->get_submap_world_pose();
    
    // //std::cout << " landmarks.size() : " << landmarks.size() << std::endl;

    // if (landmarks.empty()) {
    //     return;
    // }

    // glPointSize(1.0);

    // glColor3f(1.0f, 1.0f, 1.0f);

    // glBegin(GL_POINTS);

    // for (const auto lm : landmarks) {
    //     glVertex3f(lm[0], lm[1], lm[2]);
    // }

    // glEnd();
}

void viewer::draw_submap_poses() {
    // if(!*menu_show_submap_poses_) {
    //     return;
    // }

    // std::vector<Eigen::Vector3d> poses = mpPublisher->get_submap_poses();
    
    // //std::cout << " poses.size() : " << poses.size() << std::endl;

    // if (poses.empty()) {
    //     return;
    // }

    // glPointSize(5.0);

    // glColor3f(1.0f, 0.0f, 0.0f);

    // glBegin(GL_POINTS);

    // for (const auto p : poses) {

    //     glVertex3f(p[0], p[1], p[2]);
    // }

    // glEnd();
}

void viewer::draw_all_submap_poses() {
 //    if(!*menu_show_all_submap_poses_) {
 //        return;
 //    }

 //    visualization_msgs::SubmapArray submap = mpPublisher->get_submap_poses_and_landmarks();
    
 //    //visualization_msgs::SubmapArray submap = mpPublisher->get_curr_submap_poses_and_landmarks_from_gridmap();
    
 //    //std::cout << " poses.size() : " << poses.size() << std::endl;

	// if (submap.point_cloud.empty()) {
 //        return;
 //    } else {
 //        glPointSize(1.0);

 //        glColor3f(1.0f, 1.0f, 1.0f);

 //        glBegin(GL_POINTS);

 //        //for (const auto p : submap.landmarks) {

	// 	for (const auto p : submap.point_cloud.points()) {
	// 		glVertex3f(p.position[0], p.position[1], p.position[2]);
 //        }

 //        glEnd();
 //    }


 //    if (submap.current_landmarks.empty()) {
 //        return;
 //    } else {
 //        glPointSize(3.0);

 //        glColor3f(1.0f, 0.0f, 0.0f);

 //        glBegin(GL_POINTS);

 //        for (const auto p : submap.current_landmarks) {

 //            glVertex3f(p[0], p[1], p[2]);
 //        }

 //        glEnd();
 //    }

 //    //if(*menu_show_submap_poses_) {
 //    //    return;
 //    //}

 //    if(!submap.poses.empty()) {
 //        glPointSize(3.0);

 //        glColor3f(1.0f, 1.0f, 1.0f);

 //        glBegin(GL_POINTS);

 //        for (const auto p : submap.poses) {

 //            glVertex3f(p[0], p[1], p[2]);
 //        }

 //        glEnd();
 //    }
}

void viewer::draw_curr_submap() {
    // if(!*menu_show_current_submap_) {
    //     return;
    // }

    // visualization_msgs::SubmapArray submap = mpPublisher->get_curr_submap_poses_and_landmarks();

    // //std::cout << " poses.size() : " << poses.size() << std::endl;

    // if (submap.landmarks.empty()) {
    //     return;
    // } else {
    //     glPointSize(1.0);

    //     glColor3f(1.0f, 0.0f, 0.0f);

    //     glBegin(GL_POINTS);

    //     for (const auto p : submap.landmarks) {

    //         glVertex3f(p[0], p[1], p[2]);
    //     }

    //     glEnd();
    // }

    // if(/*!submap.poses.empty()*/0) {
    //     glPointSize(5.0);

    //     glColor3f(1.0f, 1.0f, 1.0f);

    //     glBegin(GL_POINTS);

    //     for (const auto p : submap.poses) {

    //         glVertex3f(p[0], p[1], p[2]);
    //     }

    //     glEnd();
    // }

}

void viewer::draw_trajectory_node_list() {
    // if(!*menu_show_trajectory_node_) {
    //     return;
    // }
    // trajectory_node_list_ = mpPublisher->get_trajectory_node_list();
    // if (trajectory_node_list_.markers.empty()) {
    //     return;
    // }

    // glPointSize(4.0);

    // if(!localization_status_) {
    //     glColor3f(1.0f, 0.0f, 1.0f);
    // } else {
    //     //glColor3f(0.0f, 1.0f, 1.0f);
    // }

    // glBegin(GL_POINTS);
    // for(const auto marker : trajectory_node_list_.markers) {
    //     if(marker.points.empty()) continue;
        
    //     if(localization_status_ && (marker.trajectory_id == trajectory_id_)) {
    //         glColor3f(0.0f, 1.0f, 1.0f);
    //     } else if(localization_status_) {
    //         glColor3f(1.0f, 0.0f, 1.0f);
    //     }
        
    //     for (const auto p : marker.points) {
    //         glVertex3f(p.x, p.y, p.z);
    //     }
    // }
    // glEnd();
}

void viewer::draw_constraint_list() {
    // if(!*menu_show_constraint_list_) {
    //     return;
    // }

    // constraint_list_ = mpPublisher->get_constraint_list();
    // if (constraint_list_.markers.empty()) {
    //     return;
    // }

    // //glPointSize(4.0);
    // glLineWidth(1);
    // //glColor3f(1.0f, 0.0f, 1.0f);

    // //glBegin(GL_POINTS);
    // glBegin(GL_LINES);
    // //LOG(INFO) << " constraint_list_.markers === : " << constraint_list_.markers.size();

    // auto constraint_intra_marker = constraint_list_.markers[0];
    
    // if(!constraint_intra_marker.points.empty()) {

    //     glColor3f(0.0, 1.0, 0.0); 
        
    //     const int size = constraint_intra_marker.points.size();

    //     //LOG(INFO) << " size 0 === : " << size;

    //     for(int i = 0; i <= (size - 2); i += 2) {
    //         visualization_msgs::Point ps = constraint_intra_marker.points[i];
    //         visualization_msgs::Point pe = constraint_intra_marker.points[i + 1];
    //         draw_line(ps.x, ps.y, ps.z, pe.x, pe.y, pe.z);
    //     }
    // }    

    // auto residual_intra_marker = constraint_list_.markers[1];

    // if(!residual_intra_marker.points.empty()) {

    //     glColor3f(0.0, 0.0, 1.0);

    //     const int size = residual_intra_marker.points.size();

    //     //LOG(INFO) << " size 1 === : " << size;

    //     for(int i = 0; i <= (size - 2); i += 2) {
    //         visualization_msgs::Point ps = residual_intra_marker.points[i];
    //         visualization_msgs::Point pe = residual_intra_marker.points[i + 1];
    //         draw_line(ps.x, ps.y, ps.z, pe.x, pe.y, pe.z);
    //     }        
    // }    
           
    // auto constraint_inter_same_trajectory_marker = constraint_list_.markers[2];    

    // if(!constraint_inter_same_trajectory_marker.points.empty()) {

    //     glColor3f(1.0, 1.0, 0.0); 

    //     const int size = constraint_inter_same_trajectory_marker.points.size();

    //     //LOG(INFO) << " size 2 === : " << size;

    //     for(int i = 0; i <= (size - 2); i += 2) {
    //         visualization_msgs::Point ps = constraint_inter_same_trajectory_marker.points[i];
    //         visualization_msgs::Point pe = constraint_inter_same_trajectory_marker.points[i + 1];
    //         draw_line(ps.x, ps.y, ps.z, pe.x, pe.y, pe.z);
    //     }              
    // }        

    // auto residual_inter_same_trajectory_marker = constraint_list_.markers[3];

    // if(!residual_inter_same_trajectory_marker.points.empty()) {

    //     glColor3f(0.0, 1.0, 1.0); 

    //     const int size = residual_inter_same_trajectory_marker.points.size();

    //     //LOG(INFO) << " size 3 === : " << size;

    //     for(int i = 0; i <= (size - 2); i += 2) {
    //         visualization_msgs::Point ps = residual_inter_same_trajectory_marker.points[i];
    //         visualization_msgs::Point pe = residual_inter_same_trajectory_marker.points[i + 1];
    //         draw_line(ps.x, ps.y, ps.z, pe.x, pe.y, pe.z);
    //     }            
    // }       
        
    // auto constraint_inter_diff_trajectory_marker = constraint_list_.markers[4];
    
    // if(!constraint_inter_diff_trajectory_marker.points.empty()) {
        
    //     glColor3f(1.0, 1.0, 1.0);

    //     const int size = constraint_inter_diff_trajectory_marker.points.size();
    //     for(int i = 0; i <= (size - 2); i += 2) {
    //         visualization_msgs::Point ps = constraint_inter_diff_trajectory_marker.points[i];
    //         visualization_msgs::Point pe = constraint_inter_diff_trajectory_marker.points[i + 1];
    //         draw_line(ps.x, ps.y, ps.z, pe.x, pe.y, pe.z);
    //     }            
    // }       

    // auto residual_inter_diff_trajectory_marker = constraint_list_.markers[5];

    // if(!residual_inter_diff_trajectory_marker.points.empty()) {
    //     glColor3f(1.0, 0.0, 0.0); 
    //     const int size = residual_inter_diff_trajectory_marker.points.size();
    //     for(int i = 0; i <= (size - 2); i += 2) {
    //         visualization_msgs::Point ps = residual_inter_diff_trajectory_marker.points[i];
    //         visualization_msgs::Point pe = residual_inter_diff_trajectory_marker.points[i + 1];
    //         draw_line(ps.x, ps.y, ps.z, pe.x, pe.y, pe.z);
    //     }            
    // }           

    // glEnd();
}

void viewer::draw_particle() {
	// std::vector<Eigen::Vector3f> particles = mpPublisher->get_particles();
	// if (particles.empty()) {
	// 	return;
	// }

	// glPointSize(3.0);

	// glColor3f(0.0f, 1.0f, 1.0f);

	// glBegin(GL_POINTS);

	// for (const auto p : particles) {

	// 	glVertex3f(p[0], p[1], p[2]);
	// }

	// glEnd();
}

void viewer::draw_frustum(const float w) const {
    const float h = w * 0.75f;
    const float z = w * 0.6f;
    // 四角錐の斜辺
    draw_line(0.0f, 0.0f, 0.0f, w, h, z);
    draw_line(0.0f, 0.0f, 0.0f, w, -h, z);
    draw_line(0.0f, 0.0f, 0.0f, -w, -h, z);
    draw_line(0.0f, 0.0f, 0.0f, -w, h, z);
    // 四角錐の底辺
    draw_line(w, h, z, w, -h, z);
    draw_line(-w, h, z, -w, -h, z);
    draw_line(-w, h, z, w, h, z);
    draw_line(-w, -h, z, w, -h, z);
}

} // namespace pangolin_viewer
